#include "bit.h"

#include "mem.h"
#include "assert.h"

#include <string.h>

#define T Bit_T

struct T {
    int length;
    unsigned char *bytes;
    unsigned long *words;
};

// Bit Per word
#define BPW (8 * sizeof(unsigned long))
// Number of words for len bits
#define nwords(len) (((len+BPW-1) & (~(BPW-1))) / BPW)
#define nbytes(len) (((len+8-1) & (~(8-1))) / 8)

#define bit_in_set(set, n) ((set->bytes[n/8] >> (n%8)) & 1)

// Most Significant Bit mask for bit [0, 7]
static unsigned char msbmask[] = {
    0xFF, 0xFE, 0xFC, 0xF8,
    0xF0, 0xE0, 0xC0, 0x80,
};
// Least Significant Bit mask for bit [0, 7]
static unsigned char lsbmask[] = {
    0x01, 0x03, 0x07, 0x0F,
    0x1F, 0x3F, 0x7F, 0xFF,
};

// Create a bit vector include length bits
T Bit_new(int length)
{
    assert(length >= 0);

    T set;
    
    NEW(set);
    if (length == 0)
        set->words = NULL;
    else
        set->words = CALLOC(nwords(length), sizeof(unsigned long));
    set->bytes = (unsigned char*)set->words;
    set->length = length;
    return set;
}

int Bit_length(T set)
{
    assert(set);
    return set->length;
}

int Bit_count(T set)
{
    assert(set);

    // Number of bit for a 4-bit value
    static char count[] = {
        0, 1, 1, 2,
        1, 2, 2, 3,
        1, 2, 2, 3,
        2, 3, 3, 4,
    };

    int n = 0;
    unsigned char c;
    for (int i = nbytes(set->length); --i >= 0;) {
        c = set->bytes[i];
        n += count[c & 0xF] + count[c >> 4];
    }
    return n;
}

void Bit_free(T *set)
{
    assert(set && *set);
    FREE((*set)->words);
    FREE(*set);
}

int Bit_get(T set, int n)
{
    assert(set);
    assert(n >= 0 && n < set->length);
    return bit_in_set(set, n);
}

int Bit_put(T set, int n, int bit)
{
    assert(set);
    assert(n >= 0 && n < set->length);

    int prev = bit_in_set(set, n);
    if (bit == 1)
        set->bytes[n/8] |= 1 << (n%8);
    else
        set->bytes[n/8] &= ~(1 << (n%8));
    return prev;
}

void Bit_clear(T set, int lo, int hi)
{
    assert(set);
    assert(lo >= 0 && hi < set->length);
    assert(lo <= hi);

    if (lo/8 < hi/8) {
        set->bytes[lo/8] &= ~msbmask[lo%8];
        for (int i = lo/8 + 1; i < hi/8; i++)
            set->bytes[i] = 0x00;
        set->bytes[hi/8] &= ~lsbmask[hi%8];
    } else {
        set->bytes[lo/8] &= ~(msbmask[lo%8] & lsbmask[hi%8]);
    }
}

void Bit_set(T set, int lo, int hi)
{
    assert(set);
    assert(lo >= 0 && hi < set->length);
    assert(lo <= hi);

    if (lo/8 < hi/8) {
        set->bytes[lo/8] |= msbmask[lo%8];
        for (int i = lo/8 + 1; i < hi/8; i++)
            set->bytes[i] = 0xFF;
        set->bytes[hi/8] |= lsbmask[hi%8];
    } else {
        set->bytes[lo/8] |= msbmask[lo%8] & lsbmask[hi%8]; // The same byte
    }
}

void Bit_not(T set, int lo, int hi)
{
    assert(set);
    assert(lo >= 0 && hi < set->length);
    assert(lo <= hi);

    if (lo/8 < hi/8) {
        set->bytes[lo/8] ^= msbmask[lo%8]; // Use xor for not
        for (int i = lo/8 + 1; i < hi/8; i++)
            set->bytes[i] ^= 0xFF;
        set->bytes[hi/8] ^= lsbmask[hi%8];
    } else {
        set->bytes[lo/8] ^= msbmask[lo%8] & lsbmask[hi%8];
    }
}

void Bit_map(T set, void apply(int n, int bit, void *cl), void *cl)
{
    assert(set);

    for (int n = 0; n < set->length; n++)
        apply(n, bit_in_set(set, n), cl);
}

// set s less than set t
int Bit_lt(T s, T t)
{
    assert(s && t);
    assert(s->length == t->length);

    int lt = 0;
    for (int i = nwords(s->length); --i >= 0;) {
        if ((s->words[i] & ~t->words[i]) != 0)
            return 0;
        else if (s->words[i] != t->words[i])
            lt |= 1;
    }
    return lt;
}

// set s equal to set t
int Bit_eq(T s, T t)
{
    assert(s && t);
    assert(s->length == t->length);

    for (int i = nwords(s->length); --i >= 0;) {
        if (s->words[i] != t->words[i])
            return 0;
    }
    return 1;
}

// set s less than or equal to set t
int Bit_leq(T s, T t)
{
    assert(s && t);
    assert(s->length == t->length);
    
    for (int i = nwords(s->length); --i >= 0;) {
        if ((s->words[i] & ~t->words[i]) != 0)
            return 0;
    }
    return 1;
}

static T
copy(T t)
{
    assert(t);
    
    T set = Bit_new(t->length);
    if (t->length > 0)
        memcpy(set->bytes, t->bytes, nbytes(t->length));
    return set;
}

#define setop(s, t, op) {\
    assert(s->length == t->length); \
    T set = Bit_new(s->length); \
    for (int i = nwords(s->length); --i >= 0;) \
        set->words[i] = s->words[i] op t->words[i]; \
    return set; \
}

// s + t
T Bit_union(T s, T t)
{
    assert(s || t);

    if (s == NULL)
        return copy(t);
    else if (t == NULL)
        return copy(s);
    else
        setop(s, t, |);
}

// s * t
T Bit_inter(T s, T t)
{
    assert(s || t);

    if (s == NULL)
        return Bit_new(t->length);
    else if (t == NULL)
        return Bit_new(s->length);
    else
        setop(s, t, &);
}

// s - t
T Bit_minus(T s, T t)
{
    assert(s || t);

    if (s == NULL)
        return Bit_new(t->length);
    else if (t == NULL)
        return copy(s);
    else
        setop(s, t, & ~);
}

// s / t
T Bit_diff(T s, T t)
{
    assert(s || t);

    if (s == NULL)
        return copy(t);
    else if (t == NULL)
        return copy(s);
    else
        setop(s, t, ^);
}

